/***************************************************************
 * Name:      GUIMain.cpp
 * Purpose:   Code for Application Frame
 * Author:    Robotnik Automation (mrodriguez@robotnik.es)
 * Created:   2012-03-02
 * Copyright: Robotnik Automation (http://www.robotnik.eu/)
 * License:
 **************************************************************/

#include "GUIMain.h"
#include "AutomaticCtrl.h"
#include <wx/msgdlg.h>

//(*InternalHeaders(GUIFrame)
#include <wx/string.h>
#include <wx/intl.h>
//*)
//helper functions
enum wxbuildinfoformat {
    short_f, long_f };

wxString wxbuildinfo(wxbuildinfoformat format)
{
    wxString wxbuild(wxVERSION_STRING);

    if (format == long_f )
    {
#if defined(__WXMSW__)
        wxbuild << _T("-Windows");
#elif defined(__UNIX__)
        wxbuild << _T("-Linux");
#endif

#if wxUSE_UNICODE
        wxbuild << _T("-Unicode build");
#else
        wxbuild << _T("-ANSI build");
#endif // wxUSE_UNICODE
    }

    return wxbuild;
}

//(*IdInit(GUIFrame)
const long GUIFrame::idMenuQuit = wxNewId();
const long GUIFrame::ID_MENUITEM4 = wxNewId();
const long GUIFrame::ID_MENUITEM5 = wxNewId();
const long GUIFrame::id_cam_left = wxNewId();
const long GUIFrame::id_cam_right = wxNewId();
const long GUIFrame::ID_MENUITEM2 = wxNewId();
const long GUIFrame::ID_MENUITEM3 = wxNewId();
const long GUIFrame::ID_MENUITEM1 = wxNewId();
const long GUIFrame::idMenuAbout = wxNewId();
const long GUIFrame::ID_STATUSBAR1 = wxNewId();
const long GUIFrame::ID_TIMER1 = wxNewId();
//*)

BEGIN_EVENT_TABLE(GUIFrame,wxFrame)
    //(*EventTable(GUIFrame)
    //*)
END_EVENT_TABLE()

GUIFrame::GUIFrame(wxWindow* parent,wxWindowID id)
{
    Ri = new RosInterface();
    PM = new ProcessManager();
    //(*Initialize(GUIFrame)
    wxMenuItem* MenuItem2;
    wxMenuItem* MenuItem1;
    wxMenu* Menu1;
    wxMenuBar* MenuBar1;
    wxMenu* Menu2;
    
    Create(parent, wxID_ANY, wxEmptyString, wxDefaultPosition, wxDefaultSize, wxDEFAULT_FRAME_STYLE, _T("wxID_ANY"));
    SetClientSize(wxSize(725,622));
    MenuBar1 = new wxMenuBar();
    Menu1 = new wxMenu();
    MenuItem1 = new wxMenuItem(Menu1, idMenuQuit, _("Quit\tAlt-F4"), _("Quit the application"), wxITEM_NORMAL);
    Menu1->Append(MenuItem1);
    MenuBar1->Append(Menu1, _("&File"));
    Menu9 = new wxMenu();
    Menu10 = new wxMenuItem(Menu9, ID_MENUITEM4, _("Rviz"), wxEmptyString, wxITEM_NORMAL);
    Menu9->Append(Menu10);
    Menu11 = new wxMenuItem(Menu9, ID_MENUITEM5, _("OpenRAVE"), wxEmptyString, wxITEM_NORMAL);
    Menu9->Append(Menu11);
    MenuBar1->Append(Menu9, _("Simulation"));
    Menu3 = new wxMenu();
    MenuItem3 = new wxMenuItem(Menu3, id_cam_left, _("/stereo/left/image_raw"), wxEmptyString, wxITEM_NORMAL);
    Menu3->Append(MenuItem3);
    MenuItem4 = new wxMenuItem(Menu3, id_cam_right, _("/stereo/right/image_raw"), wxEmptyString, wxITEM_NORMAL);
    Menu3->Append(MenuItem4);
    MenuBar1->Append(Menu3, _("Cams"));
    Menu6 = new wxMenu();
    Menu7 = new wxMenuItem(Menu6, ID_MENUITEM2, _("Manual"), wxEmptyString, wxITEM_NORMAL);
    Menu6->Append(Menu7);
    Menu8 = new wxMenuItem(Menu6, ID_MENUITEM3, _("Automatic"), wxEmptyString, wxITEM_NORMAL);
    Menu6->Append(Menu8);
    MenuBar1->Append(Menu6, _("Robot control"));
    Menu4 = new wxMenu();
    Menu5 = new wxMenuItem(Menu4, ID_MENUITEM1, _("Skype"), wxEmptyString, wxITEM_NORMAL);
    Menu4->Append(Menu5);
    MenuBar1->Append(Menu4, _("Call"));
    Menu2 = new wxMenu();
    MenuItem2 = new wxMenuItem(Menu2, idMenuAbout, _("About\tF1"), _("Show info about this application"), wxITEM_NORMAL);
    Menu2->Append(MenuItem2);
    MenuBar1->Append(Menu2, _("Help"));
    SetMenuBar(MenuBar1);
    StatusBar1 = new wxStatusBar(this, ID_STATUSBAR1, 0, _T("ID_STATUSBAR1"));
    int __wxStatusBarWidths_1[1] = { -1 };
    int __wxStatusBarStyles_1[1] = { wxSB_NORMAL };
    StatusBar1->SetFieldsCount(1,__wxStatusBarWidths_1);
    StatusBar1->SetStatusStyles(1,__wxStatusBarStyles_1);
    SetStatusBar(StatusBar1);
    Timer1.SetOwner(this, ID_TIMER1);
    Timer1.Start(500, false);
    
    Connect(idMenuQuit,wxEVT_COMMAND_MENU_SELECTED,(wxObjectEventFunction)&GUIFrame::OnQuit);
    Connect(ID_MENUITEM4,wxEVT_COMMAND_MENU_SELECTED,(wxObjectEventFunction)&GUIFrame::run_rviz);
    Connect(ID_MENUITEM5,wxEVT_COMMAND_MENU_SELECTED,(wxObjectEventFunction)&GUIFrame::run_OpenRAVE);
    Connect(id_cam_left,wxEVT_COMMAND_MENU_SELECTED,(wxObjectEventFunction)&GUIFrame::stereo_left_cam);
    Connect(id_cam_right,wxEVT_COMMAND_MENU_SELECTED,(wxObjectEventFunction)&GUIFrame::stereo_right_cam);
    Connect(ID_MENUITEM2,wxEVT_COMMAND_MENU_SELECTED,(wxObjectEventFunction)&GUIFrame::manual_control);
    Connect(ID_MENUITEM3,wxEVT_COMMAND_MENU_SELECTED,(wxObjectEventFunction)&GUIFrame::automatic_control);
    Connect(ID_MENUITEM1,wxEVT_COMMAND_MENU_SELECTED,(wxObjectEventFunction)&GUIFrame::skype_call);
    Connect(idMenuAbout,wxEVT_COMMAND_MENU_SELECTED,(wxObjectEventFunction)&GUIFrame::OnAbout);
    Connect(ID_TIMER1,wxEVT_TIMER,(wxObjectEventFunction)&GUIFrame::OnTimer1Trigger);
    Connect(wxID_ANY,wxEVT_CLOSE_WINDOW,(wxObjectEventFunction)&GUIFrame::OnClose);
    //*)

    int inc_x = 16;
    int inc_y = 16;
    for (int i=0; i<7; i++)
    {
        arm_name_box[i] = new wxTextCtrl(this, wxNewId(), _(""), wxPoint(inc_x,inc_y), wxSize(96,27), 0, wxDefaultValidator, _T("ID_TEXTCTRL"));
        inc_x+=100;
    }
    inc_x = 16;
    inc_y += 32;
    for (int i=0; i<7; i++)
    {
        arm_pos_box[i] = new wxTextCtrl(this, wxNewId(), _(""), wxPoint(inc_x,inc_y), wxSize(96,27), 0, wxDefaultValidator, _T("ID_TEXTCTRL"));
        inc_x+=100;
    }
    inc_x = 16;
    inc_y += 32;
    for (int i=0; i<7; i++)
    {
        arm_vel_box[i] = new wxTextCtrl(this, wxNewId(), _(""), wxPoint(inc_x,inc_y), wxSize(96,27), 0, wxDefaultValidator, _T("ID_TEXTCTRL"));
        inc_x+=100;
    }

    inc_x = 16;
    inc_y += 50;
    for (int i=0; i<7; i++)
    {
        sdh_name_box[i] = new wxTextCtrl(this, wxNewId(), _(""), wxPoint(inc_x,inc_y), wxSize(96,27), 0, wxDefaultValidator, _T("ID_TEXTCTRL"));
        inc_x+=100;
    }
    inc_x = 16;
    inc_y += 32;
    for (int i=0; i<7; i++)
    {
        sdh_pos_box[i] = new wxTextCtrl(this, wxNewId(), _(""), wxPoint(inc_x,inc_y), wxSize(96,27), 0, wxDefaultValidator, _T("ID_TEXTCTRL"));
        inc_x+=100;
    }
    inc_x = 16;
    inc_y += 32;
    for (int i=0; i<7; i++)
    {
        sdh_vel_box[i] = new wxTextCtrl(this, wxNewId(), _(""), wxPoint(inc_x,inc_y), wxSize(96,27), 0, wxDefaultValidator, _T("ID_TEXTCTRL"));
        inc_x+=100;
    }


    inc_x = 16;
    inc_y += 50;
    for (int i=0; i<3; i++)
    {
        torso_name_box[i] = new wxTextCtrl(this, wxNewId(), _(""), wxPoint(inc_x,inc_y), wxSize(130,27), 0, wxDefaultValidator, _T("ID_TEXTCTRL"));
        inc_x+=134;
    }
    inc_x = 16;
    inc_y += 32;
    for (int i=0; i<3; i++)
    {
        torso_pos_box[i] = new wxTextCtrl(this, wxNewId(), _(""), wxPoint(inc_x,inc_y), wxSize(130,27), 0, wxDefaultValidator, _T("ID_TEXTCTRL"));
        inc_x+=134;
    }
    inc_x = 16;
    inc_y += 32;
    for (int i=0; i<3; i++)
    {
        torso_vel_box[i] = new wxTextCtrl(this, wxNewId(), _(""), wxPoint(inc_x,inc_y), wxSize(130,27), 0, wxDefaultValidator, _T("ID_TEXTCTRL"));
        inc_x+=134;
    }

    inc_x = 16;
    inc_y += 50;
        tray_name_box[0] = new wxTextCtrl(this, wxNewId(), _(""), wxPoint(inc_x,inc_y), wxSize(130,27), 0, wxDefaultValidator, _T("ID_TEXTCTRL"));
        inc_x+=134;
    inc_x = 16;
    inc_y += 32;
        tray_pos_box[0] = new wxTextCtrl(this, wxNewId(), _(""), wxPoint(inc_x,inc_y), wxSize(130,27), 0, wxDefaultValidator, _T("ID_TEXTCTRL"));
        inc_x+=134;
    inc_x = 16;
    inc_y += 32;
        tray_vel_box[0] = new wxTextCtrl(this, wxNewId(), _(""), wxPoint(inc_x,inc_y), wxSize(130,27), 0, wxDefaultValidator, _T("ID_TEXTCTRL"));
        inc_x+=134;

    inc_x = 16;
    inc_y += 50;
        head_name_box[0] = new wxTextCtrl(this, wxNewId(), _(""), wxPoint(inc_x,inc_y), wxSize(130,27), 0, wxDefaultValidator, _T("ID_TEXTCTRL"));
        inc_x+=134;
    inc_x = 16;
    inc_y += 32;
        head_pos_box[0] = new wxTextCtrl(this, wxNewId(), _(""), wxPoint(inc_x,inc_y), wxSize(130,27), 0, wxDefaultValidator, _T("ID_TEXTCTRL"));
        inc_x+=134;
    inc_x = 16;
    inc_y += 32;
        head_vel_box[0] = new wxTextCtrl(this, wxNewId(), _(""), wxPoint(inc_x,inc_y), wxSize(130,27), 0, wxDefaultValidator, _T("ID_TEXTCTRL"));
        inc_x+=134;
}

GUIFrame::~GUIFrame()
{
    //(*Destroy(GUIFrame)
    //*)
}

void GUIFrame::OnQuit(wxCommandEvent& event)
{
    Close();
}

void GUIFrame::OnAbout(wxCommandEvent& event)
{
    pid_t pid_OR;
    if((pid_OR = fork()) == -1)
	{
		printf("Error in fork: %s\n", strerror(errno));
		exit(-1); //Error al ejecutar fork
	}
	else if(pid_OR == 0)
	{ //child
		if(execlp("firefox", "firefox", "www.robotnik.es", NULL) < 0)
		{
			printf("Error in exec: %s\n", strerror(errno));
			exit(1);
		}
	}
    //wxString msg = wxbuildinfo(long_f);
    //wxMessageBox(msg, _("Welcome to..."));
}

void GUIFrame::OnClose(wxCloseEvent& event)
{
    Close();
}


void GUIFrame::OnScrollBar1Scroll(wxScrollEvent& event)
{
    //int r = ScrollBar1->GetThumbPosition();
}

void GUIFrame::OnTimer1Trigger(wxTimerEvent& event)
{
    Ri->joint_status();

    RosInterface::joint_values *jva = Ri->getJointValues("arm");
    RosInterface::joint_values *jvs = Ri->getJointValues("sdh");
    RosInterface::joint_values *jvt = Ri->getJointValues("torso");
    RosInterface::joint_values *jvh = Ri->getJointValues("head");
    RosInterface::joint_values *jvtr = Ri->getJointValues("tray");

    for (int i=0; i<7; i++)
    {
        std::string res = jva[i].name;
        arm_name_box[i]->SetBackgroundColour('red');
        arm_name_box[i]->SetValue(wxString(res.c_str(), wxConvUTF8).c_str());

        res = boost::lexical_cast<std::string>(jva[i].position);
        arm_pos_box[i]->SetValue(wxString(res.c_str(), wxConvUTF8).c_str());

        res = boost::lexical_cast<std::string>(jva[i].velocity);
        arm_vel_box[i]->SetValue(wxString(res.c_str(), wxConvUTF8).c_str());
    }

    for (int i=0; i<7; i++)
    {
        std::string res = jvs[i].name;
        sdh_name_box[i]->SetBackgroundColour('red');
        sdh_name_box[i]->SetValue(wxString(res.c_str(), wxConvUTF8).c_str());

        res = boost::lexical_cast<std::string>(jvs[i].position);
        sdh_pos_box[i]->SetValue(wxString(res.c_str(), wxConvUTF8).c_str());

        res = boost::lexical_cast<std::string>(jvs[i].velocity);
        sdh_vel_box[i]->SetValue(wxString(res.c_str(), wxConvUTF8).c_str());

    }

    for (int i=0; i<3; i++)
    {
        std::string res = jvt[i].name;
        torso_name_box[i]->SetBackgroundColour('red');
        torso_name_box[i]->SetValue(wxString(res.c_str(), wxConvUTF8).c_str());//SetValue(str);

        res = boost::lexical_cast<std::string>(jvt[i].position);
        torso_pos_box[i]->SetValue(wxString(res.c_str(), wxConvUTF8).c_str());

        res = boost::lexical_cast<std::string>(jvt[i].velocity);
        torso_vel_box[i]->SetValue(wxString(res.c_str(), wxConvUTF8).c_str());
    }

    {
        std::string res = jvtr[0].name;
        tray_name_box[0]->SetBackgroundColour('red');
        tray_name_box[0]->SetValue(wxString(res.c_str(), wxConvUTF8).c_str());

        res = boost::lexical_cast<std::string>(jvtr[0].position);
        tray_pos_box[0]->SetValue(wxString(res.c_str(), wxConvUTF8).c_str());

        res = boost::lexical_cast<std::string>(jvtr[0].velocity);
        tray_vel_box[0]->SetValue(wxString(res.c_str(), wxConvUTF8).c_str());
    }

    {
        std::string res = jvh[0].name;
        head_name_box[0]->SetBackgroundColour('red');
        head_name_box[0]->SetValue(wxString(res.c_str(), wxConvUTF8).c_str());

        res = boost::lexical_cast<std::string>(jvh[0].position);
        head_pos_box[0]->SetValue(wxString(res.c_str(), wxConvUTF8).c_str());

        res = boost::lexical_cast<std::string>(jvh[0].velocity);
        head_vel_box[0]->SetValue(wxString(res.c_str(), wxConvUTF8).c_str());
    }
}

void GUIFrame::OnBitmapButton1Click(wxCommandEvent& event)
{
    GUIFrame* Frame = new GUIFrame(0);  //Aquí va la de marc
    Frame->Show();
    //wxString msg = wxbuildinfo(long_f);
    //wxMessageBox(msg, _("TARANTINOOOOOOOOOOOOOOOOOOOOORRRRRRR"));
}

void GUIFrame::stereo_left_cam(wxCommandEvent& event)
{
    //stereo_left_cam
    PM->ExecImageView("/stereo/left/image_raw");
}

void GUIFrame::stereo_right_cam(wxCommandEvent& event)
{
    //stereo_right_cam
    PM->ExecImageView("/stereo/right/image_raw");
}

void GUIFrame::skype_call(wxCommandEvent& event)
{
    //skype_call
    PM->ExecSkype();
}

void GUIFrame::manual_control(wxCommandEvent& event)
{
    //control manual
    AutomaticCtrl* Frame = new AutomaticCtrl(0, Ri, PM);
    Frame->Show();
}

void GUIFrame::automatic_control(wxCommandEvent& event)
{
    AutomaticCtrl* Frame = new AutomaticCtrl(0, Ri, PM);
    Frame->Show();
}

void GUIFrame::run_rviz(wxCommandEvent& event)
{
    PM->ExecRviz();
}

void GUIFrame::run_OpenRAVE(wxCommandEvent& event)
{
    PM->ExecOpenRAVE(1);
}
